%作业7-1 力位混合控制器
simu_step = t_ed/simu_period;
add_step = simu_step;
simu_step = 2*simu_step;
%%
%注意数据存储方式，是组号(关节1/2)- 时间 - 数据类型（加速度速度或者位置）
state_info = zeros(2,length(1:simu_step),3);
debug_info = zeros(2,length(1:simu_step),1); %现在因为存的是力矩，就只有一列了
cart_info = zeros(2,length(1:simu_step),2);

%%
initial_state = [pi/3 0 0; -pi*2/3 0 0 ];
a_dj.reset_state(initial_state);
torque_info = zeros(2,length(1:simu_step),3);
joint_all_info = zeros(2,length(1:simu_step),3);

enable_m = repmat([1 1 0 0 0],2,1);
a_pos_force_blender = doubleJointControler(a_dj,tor_motor,0.1,1,enable_m);
a_pos_force_blender.pid_reset();

y = -0.01;
fy = 10;
ky = 1e5;


con_value =[0;0];
sample_time_count = sample_period;
%%
for i = 1:simu_step
    jacobi_m = a_dj.get_jacobi(0);
    cart_value = a_dj.get_cart_value(jacobi_m);
    cart_info(:,i,:) = cart_value;
    if(sample_time_count >= sample_period)
        if(i > add_step)
            a_pos_force_blender.set_target([pos_ed 0 0 0;zeros(1,3) fy]);
        else
            a_pos_force_blender.set_target([pos_s_traj_value(i,:) 0;zeros(1,3) fy]);
        end
        if(cart_value(2,1) < 0)
            fe = [0; -cart_value(2,1)*ky];
        else
            fe = [0;0];
        end
        debug_info(:,i,:) = a_pos_force_blender.get_pos_tor_blend_con_value(a_dj,cart_value,fe,sample_period);
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    else
        debug_info(:,i,:) = debug_info(:,i-1,:);
    end
    state_info(:,i,:) = (a_dj.simu_loop(con_value,simu_period));
    sample_time_count = sample_time_count+simu_period;

end

%%


pos_traj = [pos_s_traj_value(:,:);repmat(pos_s_traj_value(end,:),simu_step-add_step,1)];


figure;
yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,cart_info(1,:,1),'-*','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,pos_traj(1:end-1,1),'r--^','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
yyaxis right

plot(0:simu_period:(simu_step-1)*simu_period,pos_traj(1:end-1,1)-cart_info(1,:,1)','r--^','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
legend('x','x target','err','Location','southeast');
title("位移x");



%%
figure;
plot(0:simu_period:(simu_step-1)*simu_period,cart_info(2,:,1),'-*','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
yline(0,'--','Color','#D95319');
legend('y','y target');
ylim([-1.05e-4  -9.5e-5 ]);
title("位移y");

%%

figure;
plot(0:simu_period:(simu_step-1)*simu_period,ky*cart_info(2,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
yline(-10,'--','Color','#A2142F');
legend('y','y target');
ylim([-10.5  -9.5])
title("y方向接触力");


%%


%
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
% hold on
% title("总力矩");
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
% hold on
% title("线性");
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,3),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
% hold on
% title("非线性")


%% 验证一下逆解的关节速度能不能完成关节空间是否对应，之所以关节速度不是平的，因为
%解得的关节位置不是线性的，合理
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,abb,'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
% legend('target','joint1');
% title("pos S traj follow  theta");
%
%
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(1,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(2,:,2),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
% legend('joint1','joint2');
% title("pos S traj follow  omega");

